<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>MRSL Motion Primitive Library: include/mpl_basis/primitive.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">MRSL Motion Primitive Library
   &#160;<span id="projectnumber">1.2</span>
   </div>
   <div id="projectbrief">A motion primitive library for generating trajectory for mobile robots</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_8b04de6a62aca813b0b7abde855cede7.html">mpl_basis</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">primitive.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="primitive_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="preprocessor">#ifndef MPL_PRIMITIVE_H</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="preprocessor">#define MPL_PRIMITIVE_H</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="data__type_8h.html">mpl_basis/data_type.h</a>&gt;</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="waypoint_8h.html">mpl_basis/waypoint.h</a>&gt;</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;</div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="math_8h.html">math.h</a>&quot;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00021"></a><span class="lineno"><a class="line" href="classPrimitive1D.html">   21</a></span>&#160;<span class="keyword">class </span><a class="code" href="classPrimitive1D.html">Primitive1D</a> {</div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;  <span class="comment">/************************* Constructors *************************/</span></div><div class="line"><a name="l00025"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">   25</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>() {}</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;</div><div class="line"><a name="l00031"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ab9d25d6f3ffcfad2bb81f9e35e033531">   31</a></span>&#160;  <a class="code" href="classPrimitive1D.html#ab9d25d6f3ffcfad2bb81f9e35e033531">Primitive1D</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a940b1af907878e3cb6d2f7694730ee01">Vec6f</a>&amp; <a class="code" href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">coeff</a>) : <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(coeff) {}</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;</div><div class="line"><a name="l00034"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a71e204be35cb684175fbc1ac603ee084">   34</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a71e204be35cb684175fbc1ac603ee084">Primitive1D</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> u) { <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, 0, 0, 0, u, <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>; }</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;</div><div class="line"><a name="l00038"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a2c5695d027bfe3d3f4d9dd1206e44bfd">   38</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a2c5695d027bfe3d3f4d9dd1206e44bfd">Primitive1D</a>(<a class="code" href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a> state, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> u) { <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, 0, 0, u, state(1), state(0); }</div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;</div><div class="line"><a name="l00042"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a58cb82ad598cdb1be1e808a0bbd8dea1">   42</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a58cb82ad598cdb1be1e808a0bbd8dea1">Primitive1D</a>(<a class="code" href="data__type_8h.html#afd53e073786661e24985c48b4ef92fcb">Vec3f</a> state, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> u) {</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;    <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, 0, u, state(2), state(1), state(0);</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;  }</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;</div><div class="line"><a name="l00048"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a1d65d067f406e2fdbc675fce17e8ee84">   48</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a1d65d067f406e2fdbc675fce17e8ee84">Primitive1D</a>(<a class="code" href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a> state, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> u) {</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, u, state(3), state(2), state(1), state(0);</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;  }</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;</div><div class="line"><a name="l00054"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ad8abb6aa1d833a6179488e928d7c5013">   54</a></span>&#160;  <a class="code" href="classPrimitive1D.html#ad8abb6aa1d833a6179488e928d7c5013">Primitive1D</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p2, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t) {</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, 0, 0, 0, (p2 - p1) / t, p1;</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;  }</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;</div><div class="line"><a name="l00060"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a37a1b40f53fc0663cd7c0726d5b4703d">   60</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a37a1b40f53fc0663cd7c0726d5b4703d">Primitive1D</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p2, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v2,</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;              <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t) {</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;    <a class="code" href="data__type_8h.html#ad2b84927631f460dbf9862f63d624e09">Mat4f</a> A;</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;    A &lt;&lt; 0, 0, 0, 1, 0, 0, 1, 0, <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) / 6, t * t / 2, t, 1, t * t / 2, t,</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;        1, 0;</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;    <a class="code" href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a> b;</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;    b &lt;&lt; p1, v1, p2, v2;</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    <a class="code" href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a> cc = A.inverse() * b;</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;    <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> &lt;&lt; 0, 0, cc(0), cc(1), cc(2), cc(3);</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;  }</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;</div><div class="line"><a name="l00073"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a2b2f041e1dc06803e3817c0ccd068afa">   73</a></span>&#160;  <a class="code" href="classPrimitive1D.html#a2b2f041e1dc06803e3817c0ccd068afa">Primitive1D</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a1, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> p2,</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;              <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> v2, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> a2, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t) {</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    <a class="code" href="data__type_8h.html#a09f49eaed626a21b73aaee4c33e6fa45">Mat6f</a> A;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;    A &lt;&lt; 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0,</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;        <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 5) / 120, <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) / 24, <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) / 6, t * t / 2, t, 1,</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;        <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) / 24, <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) / 6, t * t / 2, t, 1, 0, <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) / 6,</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;        t * t / 2, t, 1, 0, 0;</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;    <a class="code" href="data__type_8h.html#a940b1af907878e3cb6d2f7694730ee01">Vec6f</a> b;</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;    b &lt;&lt; p1, v1, a1, p2, v2, a2;</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;    <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a> = A.inverse() * b;</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;  }</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;  <span class="comment">/*************************** Member functions **************************/</span></div><div class="line"><a name="l00092"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ad17a8bcb8b6a6de3346e0f635d604dd0">   92</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#ad17a8bcb8b6a6de3346e0f635d604dd0">J</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t, <span class="keyword">const</span> <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a>&amp; control)<span class="keyword"> const </span>{</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;    <span class="comment">// i = 1, return integration of square of vel</span></div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;    <span class="keywordflow">if</span> (control == Control::VEL || control == Control::VELxYAW)</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 5184 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 9) +</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;             <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 576 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 8) +</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 252 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 168) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 7) +</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 72 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 36) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 6) +</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 20 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) / 60 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 15) *</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;                 <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 5) +</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 4 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) / 12) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) +</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 3 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) / 3) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) +</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;             <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) * t;</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;    <span class="comment">// i = 2, return integration of square of acc</span></div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control == Control::ACC || control == Control::ACCxYAW)</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 252 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 7) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 36 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 6) +</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 20 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 15) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 5) +</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 12 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 4) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) +</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 3 + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 3) * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) +</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;             <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * t;</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;    <span class="comment">// i = 3, return integration of square of jerk</span></div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control == Control::JRK || control == Control::JRKxYAW)</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 20 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 5) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 4 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) +</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;             (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2)) / 3 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) +</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;             <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * t;</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;    <span class="comment">// i = 4, return integration of square of snap</span></div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control == Control::SNP || control == Control::SNPxYAW)</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;      <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 3 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * t * t +</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;             <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * t;</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;    <span class="keywordflow">else</span></div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;      <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;  }</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;</div><div class="line"><a name="l00125"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">  125</a></span>&#160;  <a class="code" href="data__type_8h.html#a940b1af907878e3cb6d2f7694730ee01">Vec6f</a> <a class="code" href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">coeff</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>; }</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;</div><div class="line"><a name="l00128"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">  128</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 120 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 5) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 24 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) +</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;           <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 6 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) / 2 * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4) * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(5);</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;  }</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;</div><div class="line"><a name="l00134"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">  134</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 24 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 4) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 6 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) / 2 * t * t +</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;           <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3) * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(4);</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;  }</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;</div><div class="line"><a name="l00140"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">  140</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 6 * <a class="code" href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a>(t, 3) + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 2 * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2) * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3);</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;  }</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;</div><div class="line"><a name="l00145"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">  145</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 2 * t * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * t + <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2); }</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;</div><div class="line"><a name="l00152"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#abf41611f45b5e7a47f3f121daa2116f8">  152</a></span>&#160;  std::vector&lt;decimal_t&gt; <a class="code" href="classPrimitive1D.html#abf41611f45b5e7a47f3f121daa2116f8">extrema_v</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;    std::vector&lt;decimal_t&gt; roots = <a class="code" href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a>(0, <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 6, <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) / 2, <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2), <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(3));</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;    std::vector&lt;decimal_t&gt; ts;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; it : roots) {</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;      <span class="keywordflow">if</span> (it &gt; 0 &amp;&amp; it &lt; t)</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;        ts.push_back(it);</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;      <span class="keywordflow">else</span> <span class="keywordflow">if</span> (it &gt;= t)</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;        <span class="keywordflow">break</span>;</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;    }</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;    <span class="keywordflow">return</span> ts;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;  }</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;</div><div class="line"><a name="l00169"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#a3408c9bdfe45a2501c105a8f39852d54">  169</a></span>&#160;  std::vector&lt;decimal_t&gt; <a class="code" href="classPrimitive1D.html#a3408c9bdfe45a2501c105a8f39852d54">extrema_a</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;    std::vector&lt;decimal_t&gt; roots = <a class="code" href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a>(0, 0, <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) / 2, <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1), <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(2));</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;    std::vector&lt;decimal_t&gt; ts;</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; it : roots) {</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;      <span class="keywordflow">if</span> (it &gt; 0 &amp;&amp; it &lt; t)</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;        ts.push_back(it);</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;      <span class="keywordflow">else</span> <span class="keywordflow">if</span> (it &gt;= t)</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;        <span class="keywordflow">break</span>;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;    }</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;    <span class="keywordflow">return</span> ts;</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;  }</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;</div><div class="line"><a name="l00186"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ac3a4509b8b34645f1e902ebe7fe5b531">  186</a></span>&#160;  std::vector&lt;decimal_t&gt; <a class="code" href="classPrimitive1D.html#ac3a4509b8b34645f1e902ebe7fe5b531">extrema_j</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;    std::vector&lt;decimal_t&gt; ts;</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0) != 0) {</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t_sol = -<a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(1) * 2 / <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>(0);</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;      <span class="keywordflow">if</span> (t_sol &gt; 0 &amp;&amp; t_sol &lt; t) ts.push_back(t_sol);</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;    }</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;    <span class="keywordflow">return</span> ts;</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;  }</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00197"></a><span class="lineno"><a class="line" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">  197</a></span>&#160;  <a class="code" href="data__type_8h.html#a940b1af907878e3cb6d2f7694730ee01">Vec6f</a> <a class="code" href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">c</a>{Vec6f::Zero()};</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;};</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00206"></a><span class="lineno"><a class="line" href="classPrimitive.html">  206</a></span>&#160;<span class="keyword">class </span><a class="code" href="classPrimitive.html">Primitive</a> {</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00211"></a><span class="lineno"><a class="line" href="classPrimitive.html#af40ffaf02bcf580e47ef536e44a1297d">  211</a></span>&#160;  <a class="code" href="classPrimitive.html#af40ffaf02bcf580e47ef536e44a1297d">Primitive</a>() {}</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;</div><div class="line"><a name="l00220"></a><span class="lineno"><a class="line" href="classPrimitive.html#a8414a2faeebf3c39cae13e31af15bf42">  220</a></span>&#160;  <a class="code" href="classPrimitive.html#a8414a2faeebf3c39cae13e31af15bf42">Primitive</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>, <span class="keyword">const</span> <a class="code" href="data__type_8h.html#a07429e7561c286f5368c5babef590194">VecDf</a>&amp; u, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;      : t_(t), control_(p.control) {</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;    <span class="keywordflow">if</span> (control_ == Control::SNP) {</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;        <a class="code" href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a> vec;</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;        vec &lt;&lt; p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), p.<a class="code" href="structWaypoint.html#a7966bc3955c4c0f880eea35a76e4c8ed">jrk</a>(i);</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(vec, u(i));</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;      }</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::JRK) {</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(<a class="code" href="data__type_8h.html#afd53e073786661e24985c48b4ef92fcb">Vec3f</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i)), u(i));</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::ACC) {</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(<a class="code" href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i)), u(i));</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::VEL) {</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), u(i));</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::SNPxYAW) {</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;        <a class="code" href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a> vec;</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;        vec &lt;&lt; p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), p.<a class="code" href="structWaypoint.html#a7966bc3955c4c0f880eea35a76e4c8ed">jrk</a>(i);</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(vec, u(i));</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;      }</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, u(Dim));</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::JRKxYAW) {</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(<a class="code" href="data__type_8h.html#afd53e073786661e24985c48b4ef92fcb">Vec3f</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i)), u(i));</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, u(Dim));</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::ACCxYAW) {</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(<a class="code" href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i)), u(i));</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, u(Dim));</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (control_ == Control::VELxYAW) {</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), u(i));</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, u(Dim));</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;    } <span class="keywordflow">else</span></div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;      printf(<span class="stringliteral">&quot;Null Primitive, check the control set-up of the Waypoint!\n&quot;</span>);</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;  }</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;</div><div class="line"><a name="l00262"></a><span class="lineno"><a class="line" href="classPrimitive.html#a9e7832340c4550b8d1929e68d8db4266">  262</a></span>&#160;  <a class="code" href="classPrimitive.html#a9e7832340c4550b8d1929e68d8db4266">Primitive</a>(<span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; p1, <span class="keyword">const</span> <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a>&amp; p2, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;      : t_(t), control_(p1.control) {</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;    <span class="comment">// Use jrk control</span></div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;    <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRK) {</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p1.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p1.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i),</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;                              p2.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p2.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), t_);</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;    }</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;    <span class="comment">// Use acc control</span></div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACC) {</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p1.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p2.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), t_);</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;    }</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;    <span class="comment">// Use vel control</span></div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VEL) {</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), t_);</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;    }</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;    <span class="comment">// Use jrk &amp; yaw control</span></div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRKxYAW &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::JRKxYAW) {</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p1.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p1.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i),</div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;                              p2.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p2.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(i), t_);</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, p2.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, t_);</div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;    }</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;    <span class="comment">// Use acc &amp; yaw control</span></div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACCxYAW &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::ACCxYAW) {</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p1.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p2.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(i), t_);</div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, p2.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, t_);</div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;    }</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;    <span class="comment">// Use vel &amp; yaw control</span></div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (p1.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VELxYAW &amp;&amp; p2.<a class="code" href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">control</a> == Control::VELxYAW) {</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;        prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), p2.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(i), t_);</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;      pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(p1.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, p2.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a>, t_);</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;    }</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;    <span class="comment">// Null</span></div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;    <span class="keywordflow">else</span></div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;      printf(<span class="stringliteral">&quot;Null Primitive, check the control set-up of the Waypoint!\n&quot;</span>);</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;  }</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;</div><div class="line"><a name="l00309"></a><span class="lineno"><a class="line" href="classPrimitive.html#a9d51e82845161e609300bde42acbac16">  309</a></span>&#160;  <a class="code" href="classPrimitive.html#a9d51e82845161e609300bde42acbac16">Primitive</a>(<span class="keyword">const</span> <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Vec6f&gt;</a>&amp; cs, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t, <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a> control)</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;      : t_(t), control_(control) {</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) prs_[i] = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(cs[i]);</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;    <span class="keywordflow">if</span> (cs.size() == Dim + 1) pr_yaw_ = <a class="code" href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D</a>(cs[Dim]);</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;  }</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;</div><div class="line"><a name="l00321"></a><span class="lineno"><a class="line" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">  321</a></span>&#160;  <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> <a class="code" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">evaluate</a>(<a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> t)<span class="keyword"> const </span>{</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;    <a class="code" href="structWaypoint.html">Waypoint&lt;Dim&gt;</a> <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>(control_);</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; Dim; k++) {</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;      p.<a class="code" href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">pos</a>(k) = prs_[k].p(t);</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;      p.<a class="code" href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">vel</a>(k) = prs_[k].v(t);</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;      p.<a class="code" href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">acc</a>(k) = prs_[k].a(t);</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;      p.<a class="code" href="structWaypoint.html#a7966bc3955c4c0f880eea35a76e4c8ed">jrk</a>(k) = prs_[k].j(t);</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;      <span class="keywordflow">if</span> (p.<a class="code" href="structWaypoint.html#ac0023df8bacdf49f6198ef6623d678ef">use_yaw</a>) p.<a class="code" href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">yaw</a> = normalize_angle(pr_yaw_.p(t));</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;    }</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>;</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;  }</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;</div><div class="line"><a name="l00336"></a><span class="lineno"><a class="line" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">  336</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> t_; }</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;</div><div class="line"><a name="l00339"></a><span class="lineno"><a class="line" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">  339</a></span>&#160;  <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a> <a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> control_; }</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;</div><div class="line"><a name="l00345"></a><span class="lineno"><a class="line" href="classPrimitive.html#a18688523d3bea6658b3feb944ce65a22">  345</a></span>&#160;  <a class="code" href="classPrimitive1D.html">Primitive1D</a> <a class="code" href="classPrimitive.html#a18688523d3bea6658b3feb944ce65a22">pr</a>(<span class="keywordtype">int</span> k)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> prs_[k]; }</div><div class="line"><a name="l00347"></a><span class="lineno"><a class="line" href="classPrimitive.html#ab31bf042fcd6ec98a593ab6d87557fb0">  347</a></span>&#160;  <a class="code" href="classPrimitive1D.html">Primitive1D</a> <a class="code" href="classPrimitive.html#ab31bf042fcd6ec98a593ab6d87557fb0">pr_yaw</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> pr_yaw_; }</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;</div><div class="line"><a name="l00353"></a><span class="lineno"><a class="line" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">  353</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(<span class="keywordtype">int</span> k)<span class="keyword"> const </span>{</div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;    std::vector&lt;decimal_t&gt; ts = prs_[k].extrema_v(t_);</div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max_v = std::max(std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(0)), std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(t_)));</div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; it : ts) {</div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;      <span class="keywordflow">if</span> (it &gt; 0 &amp;&amp; it &lt; t_) {</div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a> = std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(it));</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;        max_v = v &gt; max_v ? <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a> : max_v;</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;      }</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;    }</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;    <span class="keywordflow">return</span> max_v;</div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;  }</div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;</div><div class="line"><a name="l00369"></a><span class="lineno"><a class="line" href="classPrimitive.html#abc3547fefaf5d3ea5421c7a4e8f9364c">  369</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#abc3547fefaf5d3ea5421c7a4e8f9364c">max_acc</a>(<span class="keywordtype">int</span> k)<span class="keyword"> const </span>{</div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;    std::vector&lt;decimal_t&gt; ts = prs_[k].extrema_a(t_);</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max_a = std::max(std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a>(0)), std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a>(t_)));</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; it : ts) {</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;      <span class="keywordflow">if</span> (it &gt; 0 &amp;&amp; it &lt; t_) {</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a> = std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a>(it));</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;        max_a = a &gt; max_a ? <a class="code" href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">a</a> : max_a;</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;      }</div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;    }</div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;    <span class="keywordflow">return</span> max_a;</div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;  }</div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;</div><div class="line"><a name="l00384"></a><span class="lineno"><a class="line" href="classPrimitive.html#a0a5a1461298a7f3ebd653faedd6866e2">  384</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#a0a5a1461298a7f3ebd653faedd6866e2">max_jrk</a>(<span class="keywordtype">int</span> k)<span class="keyword"> const </span>{</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;    std::vector&lt;decimal_t&gt; ts = prs_[k].extrema_j(t_);</div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max_j = std::max(std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a>(0)), std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a>(t_)));</div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; it : ts) {</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;      <span class="keywordflow">if</span> (it &gt; 0 &amp;&amp; it &lt; t_) {</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a> = std::abs(prs_[k].<a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a>(it));</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;        max_j = j &gt; max_j ? <a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a> : max_j;</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;      }</div><div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;    }</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;    <span class="keywordflow">return</span> max_j;</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;  }</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;</div><div class="line"><a name="l00403"></a><span class="lineno"><a class="line" href="classPrimitive.html#abebf08092f0450f9dc2796f4b231cf2a">  403</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#abebf08092f0450f9dc2796f4b231cf2a">J</a>(<span class="keyword">const</span> <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a>&amp; control)<span class="keyword"> const </span>{</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a> = 0;</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; pr : prs_) j += pr.J(t_, control);</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">j</a>;</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;  }</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;</div><div class="line"><a name="l00410"></a><span class="lineno"><a class="line" href="classPrimitive.html#ababd7cd249729297b22d28d63a7f2c49">  410</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#ababd7cd249729297b22d28d63a7f2c49">Jyaw</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> pr_yaw_.J(t_, Control::VEL); }</div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;</div><div class="line"><a name="l00415"></a><span class="lineno"><a class="line" href="classPrimitive.html#ad46e6378e08cd0f2d829787d8c44b93d">  415</a></span>&#160;  <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Waypoint&lt;Dim&gt;</a>&gt; <a class="code" href="classPrimitive.html#ad46e6378e08cd0f2d829787d8c44b93d">sample</a>(<span class="keywordtype">int</span> N)<span class="keyword"> const </span>{</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;    <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Waypoint&lt;Dim&gt;</a>&gt; ps(N + 1);</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;    <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> dt = t_ / N;</div><div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;= N; i++) ps[i] = evaluate(i * dt);</div><div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;    <span class="keywordflow">return</span> ps;</div><div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;  }</div><div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;</div><div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;  <span class="comment">/************************** Public members ************************/</span></div><div class="line"><a name="l00424"></a><span class="lineno"><a class="line" href="classPrimitive.html#a311d2629375c5805fa64ac5d930bb7b9">  424</a></span>&#160;  <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> <a class="code" href="classPrimitive.html#a311d2629375c5805fa64ac5d930bb7b9">t_</a>;</div><div class="line"><a name="l00426"></a><span class="lineno"><a class="line" href="classPrimitive.html#a64392514c8666d640bc50f6304581623">  426</a></span>&#160;  <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a> <a class="code" href="classPrimitive.html#a64392514c8666d640bc50f6304581623">control_</a>;</div><div class="line"><a name="l00428"></a><span class="lineno"><a class="line" href="classPrimitive.html#ada7fb3a9bbce1f574d3b9e49ac6631cd">  428</a></span>&#160;  std::array&lt;Primitive1D, Dim&gt; <a class="code" href="classPrimitive.html#ada7fb3a9bbce1f574d3b9e49ac6631cd">prs_</a>;</div><div class="line"><a name="l00430"></a><span class="lineno"><a class="line" href="classPrimitive.html#abd41b5f905e46efa0332fa539937627b">  430</a></span>&#160;  <a class="code" href="classPrimitive1D.html">Primitive1D</a> <a class="code" href="classPrimitive.html#abd41b5f905e46efa0332fa539937627b">pr_yaw_</a>;</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;};</div><div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;</div><div class="line"><a name="l00434"></a><span class="lineno"><a class="line" href="primitive_8h.html#ae6dd0bec01c25ea6734efc2eec0fd0f7">  434</a></span>&#160;<span class="keyword">typedef</span> <a class="code" href="classPrimitive.html">Primitive&lt;2&gt;</a> <a class="code" href="primitive_8h.html#ae6dd0bec01c25ea6734efc2eec0fd0f7">Primitive2D</a>;</div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;</div><div class="line"><a name="l00437"></a><span class="lineno"><a class="line" href="primitive_8h.html#a655244bbc3b05679f1b11732ac852e10">  437</a></span>&#160;<span class="keyword">typedef</span> <a class="code" href="classPrimitive.html">Primitive&lt;3&gt;</a> <a class="code" href="primitive_8h.html#a655244bbc3b05679f1b11732ac852e10">Primitive3D</a>;</div><div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;</div><div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;<span class="comment">/************************* Utils ******************************/</span></div><div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00450"></a><span class="lineno"><a class="line" href="primitive_8h.html#a4e7cc1831110e0cd23fa969236c8b9f5">  450</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="primitive_8h.html#a4e7cc1831110e0cd23fa969236c8b9f5">validate_primitive</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> mv = 0,</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;                        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> ma = 0, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> mj = 0,</div><div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;                        <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> myaw = 0) {</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;  <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::ACC)</div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL);</div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::JRK)</div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL) &amp;&amp;</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, ma, Control::ACC);</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::SNP)</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL) &amp;&amp;</div><div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, ma, Control::ACC) &amp;&amp;</div><div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mj, Control::JRK);</div><div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::VELxYAW)</div><div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a>(pr, myaw);</div><div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::ACCxYAW)</div><div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a>(pr, myaw) &amp;&amp; <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL);</div><div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::JRKxYAW)</div><div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a>(pr, myaw) &amp;&amp; <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL) &amp;&amp;</div><div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, ma, Control::ACC);</div><div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (pr.<a class="code" href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">control</a>() == Control::SNPxYAW)</div><div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;    <span class="keywordflow">return</span> <a class="code" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a>(pr, myaw) &amp;&amp; <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mv, Control::VEL) &amp;&amp;</div><div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, ma, Control::ACC) &amp;&amp;</div><div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;           <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(pr, mj, Control::JRK);</div><div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;  <span class="keywordflow">else</span></div><div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;}</div><div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00483"></a><span class="lineno"><a class="line" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">  483</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> max,</div><div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;                  <a class="code" href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a> xxx) {</div><div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;  <span class="keywordflow">if</span> (max &lt;= 0) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;  <span class="comment">// check if max vel is violating the constraint</span></div><div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;    <span class="keywordflow">if</span> (xxx == Control::VEL &amp;&amp; pr.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i) &gt; max)</div><div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;      <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (xxx == Control::ACC &amp;&amp; pr.<a class="code" href="classPrimitive.html#abc3547fefaf5d3ea5421c7a4e8f9364c">max_acc</a>(i) &gt; max)</div><div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;      <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (xxx == Control::JRK &amp;&amp; pr.<a class="code" href="classPrimitive.html#a0a5a1461298a7f3ebd653faedd6866e2">max_jrk</a>(i) &gt; max)</div><div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;      <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;  }</div><div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;}</div><div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;</div><div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00504"></a><span class="lineno"><a class="line" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">  504</a></span>&#160;<span class="keywordtype">bool</span> <a class="code" href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; pr, <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> my) {</div><div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;  <span class="comment">// ignore negative threshold</span></div><div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;  <span class="keywordflow">if</span> (my &lt;= 0) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;  <span class="comment">// check velocity angle at two ends, compare with my</span></div><div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;  <a class="code" href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E&lt;Waypoint&lt;Dim&gt;</a>&gt; ws(2);</div><div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;  ws[0] = pr.<a class="code" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">evaluate</a>(0);</div><div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;  ws[1] = pr.<a class="code" href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">evaluate</a>(pr.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>());</div><div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;  <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; w : ws) {</div><div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;    <span class="keyword">const</span> <span class="keyword">auto</span> <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a> = w.vel.template topRows&lt;2&gt;();</div><div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(0) != 0 || <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>(1) != 0) {  <span class="comment">// if v is not zero</span></div><div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;      <span class="comment">/*</span></div><div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;<span class="comment">      decimal_t vyaw = std::atan2(v(1), v(0));</span></div><div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;<span class="comment">      decimal_t dyaw = normalize_angle(vyaw - w.yaw);</span></div><div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;<span class="comment">      if(std::abs(dyaw) &gt; my) // if exceed the threshold</span></div><div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;<span class="comment">        return false;</span></div><div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;<span class="comment">        */</span></div><div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;      <a class="code" href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a> d = <a class="code" href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">v</a>.normalized().dot(<a class="code" href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a>(cos(w.yaw), sin(w.yaw)));</div><div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;      <span class="keywordflow">if</span> (d &lt; cos(my)) <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;    }</div><div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;  }</div><div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;}</div><div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;</div><div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00529"></a><span class="lineno"><a class="line" href="primitive_8h.html#aa3b71ca47e8823183e68290084939a66">  529</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="primitive_8h.html#aa3b71ca47e8823183e68290084939a66">print</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>) {</div><div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;Primitive: &quot;</span> &lt;&lt; std::endl;</div><div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;t: &quot;</span> &lt;&lt; p.<a class="code" href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">t</a>() &lt;&lt; std::endl;</div><div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++)</div><div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;    std::cout &lt;&lt; <span class="stringliteral">&quot;dim[&quot;</span> &lt;&lt; i &lt;&lt; <span class="stringliteral">&quot;]:     &quot;</span> &lt;&lt; p.<a class="code" href="classPrimitive.html#a18688523d3bea6658b3feb944ce65a22">pr</a>(i).<a class="code" href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">coeff</a>().transpose()</div><div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;              &lt;&lt; std::endl;</div><div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;yaw: &quot;</span> &lt;&lt; p.<a class="code" href="classPrimitive.html#ab31bf042fcd6ec98a593ab6d87557fb0">pr_yaw</a>().<a class="code" href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">coeff</a>().transpose() &lt;&lt; std::endl;</div><div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;}</div><div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;</div><div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">int</span> Dim&gt;</div><div class="line"><a name="l00540"></a><span class="lineno"><a class="line" href="primitive_8h.html#ae4c0498c7f499c8ad1157e4b7ad63c13">  540</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="primitive_8h.html#ae4c0498c7f499c8ad1157e4b7ad63c13">print_max</a>(<span class="keyword">const</span> <a class="code" href="classPrimitive.html">Primitive&lt;Dim&gt;</a>&amp; <a class="code" href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">p</a>) {</div><div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;  <a class="code" href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf&lt;Dim&gt;</a> max_v, max_a, max_j;</div><div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; Dim; i++) {</div><div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;    max_v(i) = p.<a class="code" href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">max_vel</a>(i);</div><div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;    max_a(i) = p.<a class="code" href="classPrimitive.html#abc3547fefaf5d3ea5421c7a4e8f9364c">max_acc</a>(i);</div><div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;    max_j(i) = p.<a class="code" href="classPrimitive.html#a0a5a1461298a7f3ebd653faedd6866e2">max_jrk</a>(i);</div><div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;  }</div><div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;max_vel: &quot;</span> &lt;&lt; max_v.transpose() &lt;&lt; std::endl;</div><div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;max_acc: &quot;</span> &lt;&lt; max_a.transpose() &lt;&lt; std::endl;</div><div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;  std::cout &lt;&lt; <span class="stringliteral">&quot;max_jrk: &quot;</span> &lt;&lt; max_j.transpose() &lt;&lt; std::endl;</div><div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;}</div><div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;</div><div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;<span class="preprocessor">#endif</span></div><div class="ttc" id="math_8h_html"><div class="ttname"><a href="math_8h.html">math.h</a></div><div class="ttdoc">Polynomial roots solver. </div></div>
<div class="ttc" id="classPrimitive_html_a9e7832340c4550b8d1929e68d8db4266"><div class="ttname"><a href="classPrimitive.html#a9e7832340c4550b8d1929e68d8db4266">Primitive::Primitive</a></div><div class="ttdeci">Primitive(const Waypoint&lt; Dim &gt; &amp;p1, const Waypoint&lt; Dim &gt; &amp;p2, decimal_t t)</div><div class="ttdoc">Construct from an initial state p1 and a goal state p2 for a given duration t. </div><div class="ttdef"><b>Definition:</b> primitive.h:262</div></div>
<div class="ttc" id="classPrimitive_html_abd41b5f905e46efa0332fa539937627b"><div class="ttname"><a href="classPrimitive.html#abd41b5f905e46efa0332fa539937627b">Primitive::pr_yaw_</a></div><div class="ttdeci">Primitive1D pr_yaw_</div><div class="ttdoc">Primitive for yaw. </div><div class="ttdef"><b>Definition:</b> primitive.h:430</div></div>
<div class="ttc" id="primitive_8h_html_ae6dd0bec01c25ea6734efc2eec0fd0f7"><div class="ttname"><a href="primitive_8h.html#ae6dd0bec01c25ea6734efc2eec0fd0f7">Primitive2D</a></div><div class="ttdeci">Primitive&lt; 2 &gt; Primitive2D</div><div class="ttdoc">Primitive for 2D. </div><div class="ttdef"><b>Definition:</b> primitive.h:434</div></div>
<div class="ttc" id="primitive_8h_html_a1ca288bf912779e44fad52c5335cb199"><div class="ttname"><a href="primitive_8h.html#a1ca288bf912779e44fad52c5335cb199">validate_yaw</a></div><div class="ttdeci">bool validate_yaw(const Primitive&lt; Dim &gt; &amp;pr, decimal_t my)</div><div class="ttdoc">Check if the successor goes outside of the fov. </div><div class="ttdef"><b>Definition:</b> primitive.h:504</div></div>
<div class="ttc" id="classPrimitive1D_html_a58cb82ad598cdb1be1e808a0bbd8dea1"><div class="ttname"><a href="classPrimitive1D.html#a58cb82ad598cdb1be1e808a0bbd8dea1">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(Vec3f state, decimal_t u)</div><div class="ttdef"><b>Definition:</b> primitive.h:42</div></div>
<div class="ttc" id="classPrimitive1D_html_a34ad2e359b51fafff4a1fa9f84970bc7"><div class="ttname"><a href="classPrimitive1D.html#a34ad2e359b51fafff4a1fa9f84970bc7">Primitive1D::j</a></div><div class="ttdeci">decimal_t j(decimal_t t) const</div><div class="ttdoc">Return  at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:145</div></div>
<div class="ttc" id="classPrimitive1D_html_af9558257eb00ab74376f795172068d25"><div class="ttname"><a href="classPrimitive1D.html#af9558257eb00ab74376f795172068d25">Primitive1D::a</a></div><div class="ttdeci">decimal_t a(decimal_t t) const</div><div class="ttdoc">Return  at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:140</div></div>
<div class="ttc" id="classPrimitive_html_abc3547fefaf5d3ea5421c7a4e8f9364c"><div class="ttname"><a href="classPrimitive.html#abc3547fefaf5d3ea5421c7a4e8f9364c">Primitive::max_acc</a></div><div class="ttdeci">decimal_t max_acc(int k) const</div><div class="ttdoc">Return max accleration along one axis. </div><div class="ttdef"><b>Definition:</b> primitive.h:369</div></div>
<div class="ttc" id="structWaypoint_html_a7966bc3955c4c0f880eea35a76e4c8ed"><div class="ttname"><a href="structWaypoint.html#a7966bc3955c4c0f880eea35a76e4c8ed">Waypoint::jrk</a></div><div class="ttdeci">Vecf&lt; Dim &gt; jrk</div><div class="ttdoc">jerk in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:35</div></div>
<div class="ttc" id="classPrimitive_html_abebf08092f0450f9dc2796f4b231cf2a"><div class="ttname"><a href="classPrimitive.html#abebf08092f0450f9dc2796f4b231cf2a">Primitive::J</a></div><div class="ttdeci">decimal_t J(const Control::Control &amp;control) const</div><div class="ttdoc">Return total efforts for the given duration. </div><div class="ttdef"><b>Definition:</b> primitive.h:403</div></div>
<div class="ttc" id="structWaypoint_html_ad5c2f2b8f6dba96bb93faccde844e9a8"><div class="ttname"><a href="structWaypoint.html#ad5c2f2b8f6dba96bb93faccde844e9a8">Waypoint::yaw</a></div><div class="ttdeci">decimal_t yaw</div><div class="ttdoc">yaw </div><div class="ttdef"><b>Definition:</b> waypoint.h:36</div></div>
<div class="ttc" id="structWaypoint_html"><div class="ttname"><a href="structWaypoint.html">Waypoint</a></div><div class="ttdoc">Waypoint base class. </div><div class="ttdef"><b>Definition:</b> waypoint.h:23</div></div>
<div class="ttc" id="classPrimitive1D_html"><div class="ttname"><a href="classPrimitive1D.html">Primitive1D</a></div><div class="ttdoc">Primitive1D class. </div><div class="ttdef"><b>Definition:</b> primitive.h:21</div></div>
<div class="ttc" id="classPrimitive1D_html_a9e2b1d851b9b09b7361e7f9bd1454736"><div class="ttname"><a href="classPrimitive1D.html#a9e2b1d851b9b09b7361e7f9bd1454736">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D()</div><div class="ttdoc">Empty constructor. </div><div class="ttdef"><b>Definition:</b> primitive.h:25</div></div>
<div class="ttc" id="classPrimitive1D_html_a72caa53b8186c20682493f36933fea6e"><div class="ttname"><a href="classPrimitive1D.html#a72caa53b8186c20682493f36933fea6e">Primitive1D::coeff</a></div><div class="ttdeci">Vec6f coeff() const</div><div class="ttdoc">Return coffecients. </div><div class="ttdef"><b>Definition:</b> primitive.h:125</div></div>
<div class="ttc" id="classPrimitive1D_html_ad8abb6aa1d833a6179488e928d7c5013"><div class="ttname"><a href="classPrimitive1D.html#ad8abb6aa1d833a6179488e928d7c5013">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(decimal_t p1, decimal_t p2, decimal_t t)</div><div class="ttdef"><b>Definition:</b> primitive.h:54</div></div>
<div class="ttc" id="data__type_8h_html_a07429e7561c286f5368c5babef590194"><div class="ttname"><a href="data__type_8h.html#a07429e7561c286f5368c5babef590194">VecDf</a></div><div class="ttdeci">Eigen::Matrix&lt; decimal_t, Eigen::Dynamic, 1 &gt; VecDf</div><div class="ttdoc">Eigen 1D float vector of dynamic size. </div><div class="ttdef"><b>Definition:</b> data_type.h:61</div></div>
<div class="ttc" id="classPrimitive1D_html_ab0a6b520f883c6dde48bc21cbb6a81b6"><div class="ttname"><a href="classPrimitive1D.html#ab0a6b520f883c6dde48bc21cbb6a81b6">Primitive1D::p</a></div><div class="ttdeci">decimal_t p(decimal_t t) const</div><div class="ttdoc">Return  at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:128</div></div>
<div class="ttc" id="classPrimitive_html_a8414a2faeebf3c39cae13e31af15bf42"><div class="ttname"><a href="classPrimitive.html#a8414a2faeebf3c39cae13e31af15bf42">Primitive::Primitive</a></div><div class="ttdeci">Primitive(const Waypoint&lt; Dim &gt; &amp;p, const VecDf &amp;u, decimal_t t)</div><div class="ttdoc">Construct from an initial state p and an input control u for a given duration t. </div><div class="ttdef"><b>Definition:</b> primitive.h:220</div></div>
<div class="ttc" id="classPrimitive1D_html_a37a1b40f53fc0663cd7c0726d5b4703d"><div class="ttname"><a href="classPrimitive1D.html#a37a1b40f53fc0663cd7c0726d5b4703d">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(decimal_t p1, decimal_t v1, decimal_t p2, decimal_t v2, decimal_t t)</div><div class="ttdef"><b>Definition:</b> primitive.h:60</div></div>
<div class="ttc" id="math_8h_html_a8a7fd3c51a3512fb0e8f4d70f8a93fac"><div class="ttname"><a href="math_8h.html#a8a7fd3c51a3512fb0e8f4d70f8a93fac">power</a></div><div class="ttdeci">decimal_t power(decimal_t t, int n)</div><div class="ttdoc">Return . </div><div class="ttdef"><b>Definition:</b> math.h:197</div></div>
<div class="ttc" id="classPrimitive1D_html_abf41611f45b5e7a47f3f121daa2116f8"><div class="ttname"><a href="classPrimitive1D.html#abf41611f45b5e7a47f3f121daa2116f8">Primitive1D::extrema_v</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; extrema_v(decimal_t t) const</div><div class="ttdoc">Return vector of time  for velocity extrema. </div><div class="ttdef"><b>Definition:</b> primitive.h:152</div></div>
<div class="ttc" id="math_8h_html_a981120793ddb8cdf9ce466e8c2e70e4b"><div class="ttname"><a href="math_8h.html#a981120793ddb8cdf9ce466e8c2e70e4b">solve</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; solve(decimal_t a, decimal_t b, decimal_t c, decimal_t d, decimal_t e)</div><div class="ttdoc">General solver for . </div><div class="ttdef"><b>Definition:</b> math.h:117</div></div>
<div class="ttc" id="control_8h_html_af3600af994b25be7c568791eefdadf2e"><div class="ttname"><a href="control_8h.html#af3600af994b25be7c568791eefdadf2e">Control::Control</a></div><div class="ttdeci">Control</div><div class="ttdoc">Enum for control input. </div><div class="ttdef"><b>Definition:</b> control.h:10</div></div>
<div class="ttc" id="classPrimitive_html_a2654dd3b648c5e3ee7839344a8d9e730"><div class="ttname"><a href="classPrimitive.html#a2654dd3b648c5e3ee7839344a8d9e730">Primitive::evaluate</a></div><div class="ttdeci">Waypoint&lt; Dim &gt; evaluate(decimal_t t) const</div><div class="ttdoc">Return Waypoint at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:321</div></div>
<div class="ttc" id="classPrimitive_html_a0a5a1461298a7f3ebd653faedd6866e2"><div class="ttname"><a href="classPrimitive.html#a0a5a1461298a7f3ebd653faedd6866e2">Primitive::max_jrk</a></div><div class="ttdeci">decimal_t max_jrk(int k) const</div><div class="ttdoc">Return max jerk along k-th dimension. </div><div class="ttdef"><b>Definition:</b> primitive.h:384</div></div>
<div class="ttc" id="structWaypoint_html_a648217ed387d4e226bec8ecb05bfefe8"><div class="ttname"><a href="structWaypoint.html#a648217ed387d4e226bec8ecb05bfefe8">Waypoint::acc</a></div><div class="ttdeci">Vecf&lt; Dim &gt; acc</div><div class="ttdoc">acceleration in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:34</div></div>
<div class="ttc" id="data__type_8h_html_a6b9210d48a0a541940af71827aad1b41"><div class="ttname"><a href="data__type_8h.html#a6b9210d48a0a541940af71827aad1b41">vec_E</a></div><div class="ttdeci">std::vector&lt; T, Eigen::aligned_allocator&lt; T &gt; &gt; vec_E</div><div class="ttdoc">Pre-allocated std::vector for Eigen using vec_E. </div><div class="ttdef"><b>Definition:</b> data_type.h:53</div></div>
<div class="ttc" id="data__type_8h_html_afd53e073786661e24985c48b4ef92fcb"><div class="ttname"><a href="data__type_8h.html#afd53e073786661e24985c48b4ef92fcb">Vec3f</a></div><div class="ttdeci">Vecf&lt; 3 &gt; Vec3f</div><div class="ttdoc">Eigen 1D float vector of size 3. </div><div class="ttdef"><b>Definition:</b> data_type.h:80</div></div>
<div class="ttc" id="structWaypoint_html_a32d4053f0b72324f20f1d1804fe37755"><div class="ttname"><a href="structWaypoint.html#a32d4053f0b72324f20f1d1804fe37755">Waypoint::control</a></div><div class="ttdeci">Control::Control control</div><div class="ttdoc">Control value. </div><div class="ttdef"><b>Definition:</b> waypoint.h:54</div></div>
<div class="ttc" id="structWaypoint_html_a91393e0d8d73afb9df512710e56f02a5"><div class="ttname"><a href="structWaypoint.html#a91393e0d8d73afb9df512710e56f02a5">Waypoint::vel</a></div><div class="ttdeci">Vecf&lt; Dim &gt; vel</div><div class="ttdoc">velocity in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:33</div></div>
<div class="ttc" id="classPrimitive_html_ababd7cd249729297b22d28d63a7f2c49"><div class="ttname"><a href="classPrimitive.html#ababd7cd249729297b22d28d63a7f2c49">Primitive::Jyaw</a></div><div class="ttdeci">decimal_t Jyaw() const</div><div class="ttdoc">Return total yaw efforts for the given duration. </div><div class="ttdef"><b>Definition:</b> primitive.h:410</div></div>
<div class="ttc" id="data__type_8h_html_ad2b84927631f460dbf9862f63d624e09"><div class="ttname"><a href="data__type_8h.html#ad2b84927631f460dbf9862f63d624e09">Mat4f</a></div><div class="ttdeci">Matf&lt; 4, 4 &gt; Mat4f</div><div class="ttdoc">4x4 Matrix in float </div><div class="ttdef"><b>Definition:</b> data_type.h:102</div></div>
<div class="ttc" id="classPrimitive1D_html_a2c5695d027bfe3d3f4d9dd1206e44bfd"><div class="ttname"><a href="classPrimitive1D.html#a2c5695d027bfe3d3f4d9dd1206e44bfd">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(Vec2f state, decimal_t u)</div><div class="ttdef"><b>Definition:</b> primitive.h:38</div></div>
<div class="ttc" id="classPrimitive1D_html_ac3a4509b8b34645f1e902ebe7fe5b531"><div class="ttname"><a href="classPrimitive1D.html#ac3a4509b8b34645f1e902ebe7fe5b531">Primitive1D::extrema_j</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; extrema_j(decimal_t t) const</div><div class="ttdoc">Return vector of time  for jerk extrema. </div><div class="ttdef"><b>Definition:</b> primitive.h:186</div></div>
<div class="ttc" id="classPrimitive1D_html_a439db3b1ca1eeaf9fc0500ff74ce6308"><div class="ttname"><a href="classPrimitive1D.html#a439db3b1ca1eeaf9fc0500ff74ce6308">Primitive1D::v</a></div><div class="ttdeci">decimal_t v(decimal_t t) const</div><div class="ttdoc">Return  at time . </div><div class="ttdef"><b>Definition:</b> primitive.h:134</div></div>
<div class="ttc" id="classPrimitive1D_html_a71e204be35cb684175fbc1ac603ee084"><div class="ttname"><a href="classPrimitive1D.html#a71e204be35cb684175fbc1ac603ee084">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(decimal_t p, decimal_t u)</div><div class="ttdoc">Construct 1D primitive from an initial state (p) and an input control (u) </div><div class="ttdef"><b>Definition:</b> primitive.h:34</div></div>
<div class="ttc" id="classPrimitive_html_a908ba5793fc0520602352b59358a34ea"><div class="ttname"><a href="classPrimitive.html#a908ba5793fc0520602352b59358a34ea">Primitive::t</a></div><div class="ttdeci">decimal_t t() const</div><div class="ttdoc">Return duration . </div><div class="ttdef"><b>Definition:</b> primitive.h:336</div></div>
<div class="ttc" id="primitive_8h_html_a2c969d8a20fa7dbda33f9206fedb0c38"><div class="ttname"><a href="primitive_8h.html#a2c969d8a20fa7dbda33f9206fedb0c38">validate_xxx</a></div><div class="ttdeci">bool validate_xxx(const Primitive&lt; Dim &gt; &amp;pr, decimal_t max, Control::Control xxx)</div><div class="ttdoc">Check if the max velocity magnitude is within the threshold. </div><div class="ttdef"><b>Definition:</b> primitive.h:483</div></div>
<div class="ttc" id="data__type_8h_html_a7c99d9360fc6cac2762b786e2fb52266"><div class="ttname"><a href="data__type_8h.html#a7c99d9360fc6cac2762b786e2fb52266">decimal_t</a></div><div class="ttdeci">double decimal_t</div><div class="ttdoc">Rename the float type used in lib. </div><div class="ttdef"><b>Definition:</b> data_type.h:49</div></div>
<div class="ttc" id="data__type_8h_html_a09f49eaed626a21b73aaee4c33e6fa45"><div class="ttname"><a href="data__type_8h.html#a09f49eaed626a21b73aaee4c33e6fa45">Mat6f</a></div><div class="ttdeci">Matf&lt; 6, 6 &gt; Mat6f</div><div class="ttdoc">6x6 Matrix in float </div><div class="ttdef"><b>Definition:</b> data_type.h:104</div></div>
<div class="ttc" id="classPrimitive_html_a9d51e82845161e609300bde42acbac16"><div class="ttname"><a href="classPrimitive.html#a9d51e82845161e609300bde42acbac16">Primitive::Primitive</a></div><div class="ttdeci">Primitive(const vec_E&lt; Vec6f &gt; &amp;cs, decimal_t t, Control::Control control)</div><div class="ttdoc">Construct from given coefficients and duration . </div><div class="ttdef"><b>Definition:</b> primitive.h:309</div></div>
<div class="ttc" id="classPrimitive1D_html_ab9d25d6f3ffcfad2bb81f9e35e033531"><div class="ttname"><a href="classPrimitive1D.html#ab9d25d6f3ffcfad2bb81f9e35e033531">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(const Vec6f &amp;coeff)</div><div class="ttdoc">Construct from known coefficients. </div><div class="ttdef"><b>Definition:</b> primitive.h:31</div></div>
<div class="ttc" id="classPrimitive_html_ab31bf042fcd6ec98a593ab6d87557fb0"><div class="ttname"><a href="classPrimitive.html#ab31bf042fcd6ec98a593ab6d87557fb0">Primitive::pr_yaw</a></div><div class="ttdeci">Primitive1D pr_yaw() const</div><div class="ttdoc">Get the yaw primitive. </div><div class="ttdef"><b>Definition:</b> primitive.h:347</div></div>
<div class="ttc" id="classPrimitive1D_html_a1d65d067f406e2fdbc675fce17e8ee84"><div class="ttname"><a href="classPrimitive1D.html#a1d65d067f406e2fdbc675fce17e8ee84">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(Vec4f state, decimal_t u)</div><div class="ttdef"><b>Definition:</b> primitive.h:48</div></div>
<div class="ttc" id="classPrimitive1D_html_a3408c9bdfe45a2501c105a8f39852d54"><div class="ttname"><a href="classPrimitive1D.html#a3408c9bdfe45a2501c105a8f39852d54">Primitive1D::extrema_a</a></div><div class="ttdeci">std::vector&lt; decimal_t &gt; extrema_a(decimal_t t) const</div><div class="ttdoc">Return vector of time  for acceleration extrema. </div><div class="ttdef"><b>Definition:</b> primitive.h:169</div></div>
<div class="ttc" id="classPrimitive1D_html_ad9e3d96531c774471dc56d6c54752416"><div class="ttname"><a href="classPrimitive1D.html#ad9e3d96531c774471dc56d6c54752416">Primitive1D::c</a></div><div class="ttdeci">Vec6f c</div><div class="ttdoc">Coefficients. </div><div class="ttdef"><b>Definition:</b> primitive.h:197</div></div>
<div class="ttc" id="data__type_8h_html_a135c596de9b80bec15985876ebd5036e"><div class="ttname"><a href="data__type_8h.html#a135c596de9b80bec15985876ebd5036e">Vec2f</a></div><div class="ttdeci">Vecf&lt; 2 &gt; Vec2f</div><div class="ttdoc">Eigen 1D float vector of size 2. </div><div class="ttdef"><b>Definition:</b> data_type.h:76</div></div>
<div class="ttc" id="classPrimitive_html_a6c369ebe499544a832c88b6fc4ada0e4"><div class="ttname"><a href="classPrimitive.html#a6c369ebe499544a832c88b6fc4ada0e4">Primitive::max_vel</a></div><div class="ttdeci">decimal_t max_vel(int k) const</div><div class="ttdoc">Return max velocity along one axis. </div><div class="ttdef"><b>Definition:</b> primitive.h:353</div></div>
<div class="ttc" id="data__type_8h_html_a940b1af907878e3cb6d2f7694730ee01"><div class="ttname"><a href="data__type_8h.html#a940b1af907878e3cb6d2f7694730ee01">Vec6f</a></div><div class="ttdeci">Vecf&lt; 6 &gt; Vec6f</div><div class="ttdoc">Column vector in float of size 6. </div><div class="ttdef"><b>Definition:</b> data_type.h:86</div></div>
<div class="ttc" id="data__type_8h_html"><div class="ttname"><a href="data__type_8h.html">data_type.h</a></div><div class="ttdoc">Defines all data types used in this lib. </div></div>
<div class="ttc" id="classPrimitive_html_af40ffaf02bcf580e47ef536e44a1297d"><div class="ttname"><a href="classPrimitive.html#af40ffaf02bcf580e47ef536e44a1297d">Primitive::Primitive</a></div><div class="ttdeci">Primitive()</div><div class="ttdoc">Empty constructor. </div><div class="ttdef"><b>Definition:</b> primitive.h:211</div></div>
<div class="ttc" id="structWaypoint_html_ac0023df8bacdf49f6198ef6623d678ef"><div class="ttname"><a href="structWaypoint.html#ac0023df8bacdf49f6198ef6623d678ef">Waypoint::use_yaw</a></div><div class="ttdeci">bool use_yaw</div><div class="ttdoc">If true, yaw will be used in primitive generation. </div><div class="ttdef"><b>Definition:</b> waypoint.h:52</div></div>
<div class="ttc" id="classPrimitive1D_html_a2b2f041e1dc06803e3817c0ccd068afa"><div class="ttname"><a href="classPrimitive1D.html#a2b2f041e1dc06803e3817c0ccd068afa">Primitive1D::Primitive1D</a></div><div class="ttdeci">Primitive1D(decimal_t p1, decimal_t v1, decimal_t a1, decimal_t p2, decimal_t v2, decimal_t a2, decimal_t t)</div><div class="ttdef"><b>Definition:</b> primitive.h:73</div></div>
<div class="ttc" id="primitive_8h_html_a655244bbc3b05679f1b11732ac852e10"><div class="ttname"><a href="primitive_8h.html#a655244bbc3b05679f1b11732ac852e10">Primitive3D</a></div><div class="ttdeci">Primitive&lt; 3 &gt; Primitive3D</div><div class="ttdoc">Primitive for 3D. </div><div class="ttdef"><b>Definition:</b> primitive.h:437</div></div>
<div class="ttc" id="primitive_8h_html_a4e7cc1831110e0cd23fa969236c8b9f5"><div class="ttname"><a href="primitive_8h.html#a4e7cc1831110e0cd23fa969236c8b9f5">validate_primitive</a></div><div class="ttdeci">bool validate_primitive(const Primitive&lt; Dim &gt; &amp;pr, decimal_t mv=0, decimal_t ma=0, decimal_t mj=0, decimal_t myaw=0)</div><div class="ttdoc">Check if the max velocity magnitude is within the threshold. </div><div class="ttdef"><b>Definition:</b> primitive.h:450</div></div>
<div class="ttc" id="data__type_8h_html_a3a0c45655a5e009e56634ccde0c5c575"><div class="ttname"><a href="data__type_8h.html#a3a0c45655a5e009e56634ccde0c5c575">Vecf</a></div><div class="ttdeci">Eigen::Matrix&lt; decimal_t, N, 1 &gt; Vecf</div><div class="ttdoc">Eigen 1D float vector of size N. </div><div class="ttdef"><b>Definition:</b> data_type.h:56</div></div>
<div class="ttc" id="data__type_8h_html_a885809dc84c0c55d44fe4836f5cfa39b"><div class="ttname"><a href="data__type_8h.html#a885809dc84c0c55d44fe4836f5cfa39b">Vec4f</a></div><div class="ttdeci">Vecf&lt; 4 &gt; Vec4f</div><div class="ttdoc">Eigen 1D float vector of size 4. </div><div class="ttdef"><b>Definition:</b> data_type.h:84</div></div>
<div class="ttc" id="waypoint_8h_html"><div class="ttname"><a href="waypoint_8h.html">waypoint.h</a></div><div class="ttdoc">Waypoint classes. </div></div>
<div class="ttc" id="classPrimitive_html_ad46e6378e08cd0f2d829787d8c44b93d"><div class="ttname"><a href="classPrimitive.html#ad46e6378e08cd0f2d829787d8c44b93d">Primitive::sample</a></div><div class="ttdeci">vec_E&lt; Waypoint&lt; Dim &gt; &gt; sample(int N) const</div><div class="ttdoc">Sample N+1 Waypoints using uniformed time. </div><div class="ttdef"><b>Definition:</b> primitive.h:415</div></div>
<div class="ttc" id="classPrimitive_html_a18688523d3bea6658b3feb944ce65a22"><div class="ttname"><a href="classPrimitive.html#a18688523d3bea6658b3feb944ce65a22">Primitive::pr</a></div><div class="ttdeci">Primitive1D pr(int k) const</div><div class="ttdoc">Get the 1D primitive. </div><div class="ttdef"><b>Definition:</b> primitive.h:345</div></div>
<div class="ttc" id="primitive_8h_html_ae4c0498c7f499c8ad1157e4b7ad63c13"><div class="ttname"><a href="primitive_8h.html#ae4c0498c7f499c8ad1157e4b7ad63c13">print_max</a></div><div class="ttdeci">void print_max(const Primitive&lt; Dim &gt; &amp;p)</div><div class="ttdoc">Print max dynamic infomation in primitive p. </div><div class="ttdef"><b>Definition:</b> primitive.h:540</div></div>
<div class="ttc" id="classPrimitive_html_a64392514c8666d640bc50f6304581623"><div class="ttname"><a href="classPrimitive.html#a64392514c8666d640bc50f6304581623">Primitive::control_</a></div><div class="ttdeci">Control::Control control_</div><div class="ttdoc">Control. </div><div class="ttdef"><b>Definition:</b> primitive.h:426</div></div>
<div class="ttc" id="classPrimitive_html"><div class="ttname"><a href="classPrimitive.html">Primitive</a></div><div class="ttdoc">Primitive class. </div><div class="ttdef"><b>Definition:</b> primitive.h:206</div></div>
<div class="ttc" id="classPrimitive_html_a311d2629375c5805fa64ac5d930bb7b9"><div class="ttname"><a href="classPrimitive.html#a311d2629375c5805fa64ac5d930bb7b9">Primitive::t_</a></div><div class="ttdeci">decimal_t t_</div><div class="ttdoc">Duration. </div><div class="ttdef"><b>Definition:</b> primitive.h:424</div></div>
<div class="ttc" id="structWaypoint_html_af63de420a521f105eea4ce098a51ad7e"><div class="ttname"><a href="structWaypoint.html#af63de420a521f105eea4ce098a51ad7e">Waypoint::pos</a></div><div class="ttdeci">Vecf&lt; Dim &gt; pos</div><div class="ttdoc">position in  </div><div class="ttdef"><b>Definition:</b> waypoint.h:32</div></div>
<div class="ttc" id="classPrimitive_html_aa9c00e6dfba12ffc83523e010881ff8f"><div class="ttname"><a href="classPrimitive.html#aa9c00e6dfba12ffc83523e010881ff8f">Primitive::control</a></div><div class="ttdeci">Control::Control control() const</div><div class="ttdoc">Get the control indicator. </div><div class="ttdef"><b>Definition:</b> primitive.h:339</div></div>
<div class="ttc" id="classPrimitive_html_ada7fb3a9bbce1f574d3b9e49ac6631cd"><div class="ttname"><a href="classPrimitive.html#ada7fb3a9bbce1f574d3b9e49ac6631cd">Primitive::prs_</a></div><div class="ttdeci">std::array&lt; Primitive1D, Dim &gt; prs_</div><div class="ttdoc">By default, primitive class contains Dim 1D primitive. </div><div class="ttdef"><b>Definition:</b> primitive.h:428</div></div>
<div class="ttc" id="classPrimitive1D_html_ad17a8bcb8b6a6de3346e0f635d604dd0"><div class="ttname"><a href="classPrimitive1D.html#ad17a8bcb8b6a6de3346e0f635d604dd0">Primitive1D::J</a></div><div class="ttdeci">decimal_t J(decimal_t t, const Control::Control &amp;control) const</div><div class="ttdoc">Return total efforts of 1D primitive for the given duration: . </div><div class="ttdef"><b>Definition:</b> primitive.h:92</div></div>
<div class="ttc" id="primitive_8h_html_aa3b71ca47e8823183e68290084939a66"><div class="ttname"><a href="primitive_8h.html#aa3b71ca47e8823183e68290084939a66">print</a></div><div class="ttdeci">void print(const Primitive&lt; Dim &gt; &amp;p)</div><div class="ttdoc">Print all coefficients in primitive p. </div><div class="ttdef"><b>Definition:</b> primitive.h:529</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
</body>
</html>
